package com.ai.vr.service;

import java.io.IOException;

/**
 * VR 控制机器人
 * @author houwenpeng
 */
public class VRControlledRobot {

    private static final int k_unTrackedDeviceIndex_Hmd = 0; // HMD 设备的索引

    private static final String robotIp = "0.0.0.0"; // 默认 IP 地址
    private static final int robotPort = 8888; // 默认端口号

    public static void main(String[] args) throws IOException, InterruptedException {

        // 初始化OpenVR
        OpenVRDataCollector dataCollector = new OpenVRDataCollector();

        // 初始化机器人控制对象
        RobotController robotController = new RobotController(robotIp, robotPort);

        try {
            // 主循环，获取姿态并发送控制指令
            while (true) {
                // 获取 HMD 姿态数据
                float[] hmdPose = dataCollector.getHmdPose();
                if (hmdPose != null) {

                    // 将 HMD 姿态数据转换为机器人控制指令
                    String deviceModel = OpenVR.INSTANCE.VRSystem_GetStringTrackedDeviceProperty(k_unTrackedDeviceIndex_Hmd, 6000, 0);
                    String command = convertPoseToCommand(hmdPose, deviceModel);

                    // 获取手柄的输入
                    VRControllerState_t controllerState = dataCollector.getControllerState(1);
                    if (controllerState != null) {
                        // 处理手柄输入（例如按钮按下、触发器状态等）
                        processControllerInput(controllerState, robotController);
                    }

                    // 向机器人发送控制指令
                    robotController.sendCommand(command);
                }
                Thread.sleep(100);  // 100ms 延时，减少 CPU 占用
            }
        } catch (InterruptedException e) {
            e.printStackTrace();
        } finally {
            // 程序结束时关闭OpenVR和机器人控制对象
            dataCollector.shutdown();
            new RobotController(robotIp, robotPort).close();
        }
    }

    /**
     * 处理手柄输入
     * @param controllerState  手柄状态
     * @param robotController 机器人控制对象
     * @throws IOException
     * @throws InterruptedException
     */
    private static void processControllerInput(VRControllerState_t controllerState, RobotController robotController) throws IOException, InterruptedException {
        // 处理手柄输入
        if (controllerState.buttonPressed != 0) {
            System.out.println("按钮按下：" + controllerState.buttonPressed);
            // 根据按钮按下状态触发机器人不同的行为
            switch (controllerState.buttonPressed) {
                case 1:
                    robotController.sendCommand("MOVE_UP"); // 假设 "MOVE_UP" 是机器人控制命令
                    break;
                case 2:
                    robotController.sendCommand("MOVE_DOWN"); // 假设 "MOVE_DOWN" 是机器人控制命令
                    break;
                default:
                    break;
            }
        }
    }

    /**
     * 将 HMD 的姿态数据转换为机器人控制指令
     * @param pose   姿态数据
     * @param deviceModel   设备型号
     * @return
     */
    private static String convertPoseToCommand(float[] pose, String deviceModel) {
        // 根据 HMD 姿态数据来生成命令
        return String.format("MOVE %.2f %.2f %.2f", pose[12], pose[13], pose[14]); // X, Y, Z 坐标
    }
}